#include "UserApi.h"
#include "VEXRobot.h"

static unsigned char    EncoderInterrupt;
static unsigned long    EncoderDataTickCounter[6];

/*******************************************************************************
* FUNCTION NAME: EncoderInterruptHandler
* PURPOSE:       Start accepting data from the encoder
* CALLED FROM:   Anywhere
* ARGUMENTS:     ucEncNum - interrupt to use
* RETURNS:       void
*******************************************************************************/
static unsigned char priorInterruptsHit = 0xff;
void Handle_Encoder_Interrupt(unsigned char intHit)
{
    if( 0 == (EncoderInterrupt & intHit))
        return;

    switch(intHit)
    {
    case 0x01:
        EncoderDataTickCounter[0]++;
        return;
    case 0x02:
        EncoderDataTickCounter[1]++;
        return;
    case 0x04:
        EncoderDataTickCounter[2]++;
        return;
    case 0x08:
        EncoderDataTickCounter[3]++;
        return;
    case 0x10:
        EncoderDataTickCounter[4]++;
        return;
    case 0x20:
        EncoderDataTickCounter[5]++;
        return;
    }
}

/*******************************************************************************
* FUNCTION NAME: StartEncoder
* PURPOSE:       Start accepting data from the encoder
* CALLED FROM:   Anywhere
* ARGUMENTS:     ucEncNum - interrupt to use
* RETURNS:       void
*******************************************************************************/
void StartEncoder(unsigned char ucEncNum)
{
    if(ucEncNum > 0 && ucEncNum <= 6)
        EncoderInterrupt |= 1 << (ucEncNum - 1);
    EncoderDataTickCounter[ucEncNum-1] = 0;

    SetupInterruptHandler(ucEncNum, Handle_Encoder_Interrupt);
}

/*******************************************************************************
* FUNCTION NAME: PresetEncoder
* PURPOSE:       Set the encoder starting value
* CALLED FROM:   Anywhere
* ARGUMENTS:     ucEncNum - interrupt to use
*				 lValue - value to preset count to
* RETURNS:       void
*******************************************************************************/
void PresetEncoder(unsigned char ucEncNum, unsigned long lValue)
{
    if( EncoderInterrupt & (1 << (ucEncNum - 1)) )
        EncoderDataTickCounter[ucEncNum-1] = lValue;
}

/*******************************************************************************
* FUNCTION NAME: GetEncoder
* PURPOSE:       Get the current count from the encoder
* CALLED FROM:   Anywhere
* ARGUMENTS:     ucEncNum - interrupt to use
* RETURNS:       unsigned long
*******************************************************************************/
unsigned long GetEncoder(unsigned char ucEncNum)
{
    if( EncoderInterrupt & (1 << (ucEncNum - 1)) )
        return EncoderDataTickCounter[ucEncNum-1];
    return 0;
}

/*******************************************************************************
* FUNCTION NAME: StopEncoder
* PURPOSE:       Stop accepting data from the encoder
* CALLED FROM:   Anywhere
* ARGUMENTS:     ucEncNum - interrupt to use
* RETURNS:       void
*******************************************************************************/
void StopEncoder(unsigned char ucEncNum)
{
    EncoderInterrupt &= ~(1 << (ucEncNum - 1));
    SetupInterruptHandler(ucEncNum, 0);
}
